#include "ball_mapping.h"

#include<vector>
using namespace std;

static ball_info_t prev_ball_info = {0};

static inline com_point_t calcu_cam_coord(ball_map_ctx_t *ctx, point_t pt0, int view0, point_t pt1, int view1) {
    com_cam_t* cam0 = &ctx->cams[view0];
    com_cam_t* cam1 = &ctx->cams[view1];
    pt0.x *= ctx->cam_width;
    pt0.y *= ctx->cam_height;
    //std::cout << "=== cam width " << ctx->cam_width << " cam height " << ctx->cam_height << std::endl;
    pt1.x *= ctx->cam_width;
    pt1.y *= ctx->cam_height;

    com_point_t pos0 = { 
        (pt0.x - (cam0->width  * 0.5f)) / cam0->fx, 
        (pt0.y - (cam0->height * 0.5f)) / cam0->fy, 
        1.0f
    };
    com_point_t pos1 = { 
        (pt1.x - (cam1->width  * 0.5f)) / cam1->fx, 
        (pt1.y - (cam1->height * 0.5f)) / cam1->fy, 
        1.0f
    };
    
    pos0 = M4x3_x_Point(&cam0->matrix_c_2_w[0][0], 4, pos0);
    pos1 = M4x3_x_Point(&cam1->matrix_c_2_w[0][0], 4, pos1);

    //printf("View%d 2D: (%.3f, %.3f) -> Camera3D: (%.3f, %.3f, %.3f)\n", 
    //       view0, pt0.x, pt0.y, pos0.x, pos0.y, pos0.z);
    //printf("View%d 2D: (%.3f, %.3f) -> Camera3D: (%.3f, %.3f, %.3f)\n", 
    //       view1, pt1.x, pt1.y, pos1.x, pos1.y, pos1.z);

    com_point_t camPos0 = M4x3_x_Point(&cam0->matrix_c_2_w[0][0], 4, (com_point_t) { 0, 0, 0 });
    com_point_t camPos1 = M4x3_x_Point(&cam1->matrix_c_2_w[0][0], 4, (com_point_t) { 0, 0, 0 });
   
    com_point_t result = get_middle_point_of_2_lines(pos0, camPos0, pos1, camPos1);
    //printf("Triangulated 3D: (%.3f, %.3f, %.3f)\n", result.x, result.y, result.z); 
    //printf("Raw 3D result: (%.3f, %.3f, %.3f)\n", result.x, result.y, result.z);
    //printf("Camera positions - Cam0: (%.3f, %.3f, %.3f), Cam1: (%.3f, %.3f, %.3f)\n", 
    //   camPos0.x, camPos0.y, camPos0.z, camPos1.x, camPos1.y, camPos1.z);
    return result;
}

static inline float distance(point_t p0, point_t p1) {
    return sqrt((p0.x - p1.x) * (p0.x - p1.x) + (p0.y - p1.y) * (p0.y - p1.y));
}

static com_point_t get_point_cam_coord(ball_map_ctx_t *ctx, int pt_id, int flags) {
    ball_map_config_view_t *cfg_views = ctx->cfg_views;
    int view_cnt = ctx->view_cnt;
    float avg_x = 0;
    float avg_y = 0;
    float avg_z = 0;
    int count = 0;

    for (int i = 0; i < view_cnt; i++) {
        int view0 = cfg_views[i].view_id;
        point_t *pt0;
        if (flags == 0)
            pt0 = &cfg_views[i].corner_points[pt_id];
        else
            pt0 = &cfg_views[i].basket_rims_point[pt_id];
        for (int j = i + 1; j < view_cnt; j++) {
            int view1 = cfg_views[j].view_id;
            point_t *pt1;
            if (flags == 0)
                pt1 = &cfg_views[j].corner_points[pt_id];
            else
                pt1 = &cfg_views[j].basket_rims_point[pt_id];
            //std::cout << "view0:" << view0 << " view1:" << view1 << std::endl;
            com_point_t pt = calcu_cam_coord(ctx, *pt0, view0, *pt1, view1);
            //if (flags == 0)
            //    printf("({%d, %f, %f}, {%d, %f, %f} -> {%f, %f, %f} \n", view0, pt0->x, pt0->y, view1, pt1->x, pt1->y, pt.x, pt.y, pt.z);
            avg_x += pt.x;
            avg_y += pt.y;
            avg_z += pt.z;
            count++;
        }
    }
    if (count <= 0) 
        return {0, 0, 0};
    else
        return {avg_x / count, avg_y / count, avg_z / count};
}
// flags: 0(4 corner points), 1(2 basket rim points)
static void get_cam_coords(ball_map_ctx_t *ctx, int flags) {
    if (flags == 0) {
        for (int i = 0; i < 4; i++) {
            ctx->points[i] = get_point_cam_coord(ctx, i, flags);
            printf("=== corner_points[%d] = (%f, %f, %f)\n", i, ctx->points[i].x, ctx->points[i].y, ctx->points[i].z);
        }
    } else {
        for (int i = 0; i < 2; i++) {
            ctx->rim_points[i] = get_point_cam_coord(ctx, i, flags);
            //ctx->rim_points[i] = get_point_cam_coord(ctx, i, 0);
            printf("=== rim_point[%d] = (%f, %f, %f)\n", i, ctx->rim_points[i].x, ctx->rim_points[i].y, ctx->rim_points[i].z);
        }
    }
}

static point_t get_out_img_coord(ball_map_ctx_t *ctx, com_point_t *pt_cam) {
    int out_stream = ctx->out_stream_id;
    //std::cout << "get_out_img_coord: out_steam " << out_stream << std::endl;
    int out_view = ctx->cfg_views[out_stream].view_id;
    //std::cout << "get_out_img_coord: out_view " << out_view << std::endl;
    com_cam_t* cam = &ctx->cams[out_view];
    com_point_t pos = M4x3_x_Point(&cam->matrix_w_2_c[0][0], 4, *pt_cam);
    float pic_x = pos.x / pos.z * cam->fx / cam->width + 0.5f;
    float pic_y = pos.y / pos.z * cam->fy / cam->height + 0.5f;
    return {pic_x, pic_y};
}

// 使用四个角点计算球场中心点
static com_point_t get_court_center(ball_map_ctx_t *ctx) {
    com_point_t center = {0, 0, 0};
    
    // 方法1：四个角点的平均值
    for (int i = 0; i < 4; i++) {
        //printf("get_court_center Point[%d]: (%.3f, %.3f, %.3f)\n", i, ctx->points[i].x, ctx->points[i].y, ctx->points[i].z);
        center.x += ctx->points[i].x;
        center.y += ctx->points[i].y;
        center.z += ctx->points[i].z;
    }
    center.x /= 4.0;
    center.y /= 4.0;
    center.z /= 4.0;
    
    // 方法2：对角线中点（用于验证）
    com_point_t diag1_center = point_avg(ctx->points[0], ctx->points[2]); // 左上-右下对角线
    com_point_t diag2_center = point_avg(ctx->points[1], ctx->points[3]); // 右上-左下对角线
    
    // 输出调试信息
    //printf("Court corners:\n");
    //for (int i = 0; i < 4; i++) {
    //    printf("  Point[%d]: (%.3f, %.3f, %.3f)\n", 
    //           i, ctx->points[i].x, ctx->points[i].y, ctx->points[i].z);
    //}
    //printf("Center from avg: (%.3f, %.3f, %.3f)\n", center.x, center.y, center.z);
    //printf("Center from diag1: (%.3f, %.3f, %.3f)\n", diag1_center.x, diag1_center.y, diag1_center.z);
    //printf("Center from diag2: (%.3f, %.3f, %.3f)\n", diag2_center.x, diag2_center.y, diag2_center.z);
    
    return center;
}

// 计算球相对于球场中心的偏移
void calcu_ball_court_center_offset(ball_map_ctx_t *ctx, ball_info_t *ball) {
    // 获取球场中心点
    com_point_t court_center = get_court_center(ctx);
    
    //printf("Before sub center (%.3f, %.3f, %.3f)\n", ball->pos.pos_3d.x, ball->pos.pos_3d.y, ball->pos.pos_3d.z);
    //printf("Court center: (%.3f, %.3f, %.3f)\n", court_center.x, court_center.y, court_center.z);

    // 计算从中心点到球的向量
    com_vector_t vec_to_ball = point_sub_point(ball->pos.pos_3d, court_center);
    
    //std::cout << "ctx->vec_x: " << ctx->vec_x.x <<"," <<ctx->vec_x.y<<"," <<ctx->vec_x.z << std::endl;

    // 投影到球场坐标系
    float offset_x = dot(vec_to_ball, ctx->vec_x) / ctx->cam_unit_x;
    float offset_y = dot(vec_to_ball, ctx->vec_y) / ctx->cam_unit_y;
    float offset_z = dot(vec_to_ball, ctx->vec_z) / ctx->cam_unit_z * -1;
    
    ball->court_relative_pos = {offset_x, offset_y, offset_z};
    //ball->pos.pos_3d = {vec_to_ball.x, vec_to_ball.y, vec_to_ball.z};
        
    printf("=== Court Center Offset ===\n");
    printf("Offset from Center: X=%.3fm, Y=%.3fm, Z=%.3fm\n",offset_x, offset_y, offset_z);
    // 计算球到中心点的实际距离
    //float distance_to_center = dist(ball->pos.pos_3d, court_center);

    //printf("Court Center 3D: (%.3f, %.3f, %.3f)\n", 
    //       court_center.x, court_center.y, court_center.z);
    //printf("Ball Position 3D: (%.3f, %.3f, %.3f)\n", 
    //       ball->pos.pos_3d.x, ball->pos.pos_3d.y, ball->pos.pos_3d.z);
    //printf("Offset from Center: X=%.3fm, Y=%.3fm, Z=%.3fm\n", 
    //       offset_x, offset_y, offset_z);
    //printf("Distance to Center: %.3fm\n", distance_to_center);
}

static void calcu_ball_rim_vectors(ball_map_ctx_t *ctx, ball_info_t *ball) {
    for (int i = 0; i < 2; i++) {
        com_vector_t vec = point_sub_point(ctx->rim_points[i], ball->pos.pos_3d);
        float x = dot(vec, ctx->vec_x) / ctx->cam_unit_x;
        float y = dot(vec, ctx->vec_y) / ctx->cam_unit_y;
        float z = dot(vec, ctx->vec_z) / ctx->cam_unit_z;
        //printf("rim %d: (%f, %f, %f)\n", i, x, y, z);
        ball->rim_dists[i] = {x, y, z};
    }
}

// 检查点是否在球场边界内
static int is_point_in_court_boundary(ball_map_ctx_t *ctx, com_point_t point) {
    // 获取球场边界（基于四个角点）
    com_point_t court_corners[4];
    for (int i = 0; i < 4; i++) {
        court_corners[i] = ctx->points[i];
    }
    
    // 计算球场在X-Y平面的投影边界
    float min_x = court_corners[0].x, max_x = court_corners[0].x;
    float min_y = court_corners[0].y, max_y = court_corners[0].y;
    float min_z = court_corners[0].z, max_z = court_corners[0].z;
    
    for (int i = 1; i < 4; i++) {
        min_x = fmin(min_x, court_corners[i].x);
        max_x = fmax(max_x, court_corners[i].x);
        min_y = fmin(min_y, court_corners[i].y);
        max_y = fmax(max_y, court_corners[i].y);
        min_z = fmin(min_z, court_corners[i].z);
        max_z = fmax(max_z, court_corners[i].z);
    }
    
    // 添加安全边界（避免边缘点的误判）
    float safety_margin_x = (max_x - min_x) * 0.05f;
    float safety_margin_y = (max_y - min_y) * 0.05f;
    float safety_margin_z = (max_z - min_z) * 0.1f;
    
    // 检查点是否在边界内
    int in_boundary = (point.x >= (min_x - safety_margin_x) && 
                      point.x <= (max_x + safety_margin_x) &&
                      point.y >= (min_y - safety_margin_y) && 
                      point.y <= (max_y + safety_margin_y) &&
                      point.z >= (min_z - safety_margin_z) && 
                      point.z <= (max_z + safety_margin_z));
    
    printf("Point (%.3f, %.3f, %.3f) boundary check: [%.3f-%.3f, %.3f-%.3f, %.3f-%.3f] -> %s\n",
           point.x, point.y, point.z,
           min_x, max_x, min_y, max_y, min_z, max_z,
           in_boundary ? "INSIDE" : "OUTSIDE");
    
    return in_boundary;
}

// 检查点是否在合理的羽毛球飞行高度范围内
static int is_point_in_reasonable_height(com_point_t point) {
    // 羽毛球合理飞行高度范围：地面以上0.1米到10米
    const float MIN_HEIGHT = 0.0f;
    const float MAX_HEIGHT = 10.0f;
    
    // 假设地面在Z=0平面（根据实际情况调整）
    int valid_height = (point.z >= MIN_HEIGHT && point.z <= MAX_HEIGHT);
    
    if (!valid_height) {
        printf("Point height %.3f is outside reasonable range [%.1f-%.1f]\n", 
               point.z, MIN_HEIGHT, MAX_HEIGHT);
    }
    
    return valid_height;
}

// 检查点是否在网的两侧（避免羽毛球出现在网的另一侧）
static int is_point_on_correct_side_of_net(ball_map_ctx_t *ctx, com_point_t point) {
    // 获取球场中心点
    com_point_t court_center = get_court_center(ctx);
    
    // 假设网在球场中心（Y方向）
    // 如果球在场地的一侧，那么它应该出现在对应的半场
    // 这里可以根据篮筐位置来更精确地判断
    
    // 简单检查：点应该在球场范围内
    // 更复杂的检查可以基于篮筐位置和运动轨迹
    
    return 1; // 暂时返回true，可根据需要完善
}

// 基于速度的连续性检查（如果有多帧数据）
static int is_motion_continuous(com_point_t current_pos, com_point_t prev_pos, 
                               float max_speed = 50.0f) { // 最大速度50m/s
    if (prev_pos.x == 0 && prev_pos.y == 0 && prev_pos.z == 0) {
        return 1; // 第一帧数据
    }
    
    float distance = sqrt((current_pos.x - prev_pos.x) * (current_pos.x - prev_pos.x) +
                         (current_pos.y - prev_pos.y) * (current_pos.y - prev_pos.y) +
                         (current_pos.z - prev_pos.z) * (current_pos.z - prev_pos.z));
    
    // 假设帧率为30fps，则最大合理位移
    float max_distance = max_speed / 30.0f;
    
    int continuous = (distance <= max_distance);
    
    if (!continuous) {
        printf("Motion discontinuity detected: distance = %.3f, max = %.3f\n", 
               distance, max_distance);
    }
    
    return continuous;
}

// 主要的几何一致性检查函数
static int check_geometry_consistency(ball_map_ctx_t *ctx, ball_info_t *ball, 
                                     ball_info_t *prev_ball = nullptr) {
    if (!ball || !ball->avail) {
        return 0;
    }
    
    com_point_t ball_pos = ball->pos.pos_3d;
    int valid_count = 0;
    int total_checks = 3; // 总共的检查项数
    
    // 检查1：边界检查
    int boundary_ok = is_point_in_court_boundary(ctx, ball_pos);
    valid_count += boundary_ok;
    
    // 检查2：高度合理性检查
    int height_ok = is_point_in_reasonable_height(ball_pos);
    valid_count += height_ok;
    
    // 检查3：网侧检查
    int side_ok = is_point_on_correct_side_of_net(ctx, ball_pos);
    valid_count += side_ok;
    
    // 检查4：运动连续性检查（如果有前一帧数据）
    //int motion_ok = 1;
    //if (prev_ball && prev_ball->avail) {
    //    motion_ok = is_motion_continuous(ball_pos, prev_ball->pos.pos_3d);
    //    valid_count += motion_ok;
    //    total_checks++;
    //}
    
    // 设置有效性标志
    // 如果通过所有检查，或者通过大部分检查，则认为有效
    float pass_rate = (float)valid_count / total_checks;
    int is_valid = (pass_rate >= 0.25f); // 通过75%的检查
    
    printf("Geometry consistency check: %d/%d passed, result: %s\n", 
           valid_count, total_checks, is_valid ? "VALID" : "INVALID");
    
    return is_valid;
}

static int check_multi_view_consistency(ball_map_ctx_t *ctx, ball_result_t *res_views, 
                                       com_point_t estimated_3d_pos) {
    int consistent_views = 0;
    int total_views = 0;
    float total_reprojection_error = 0.0f;
    ball_map_config_view_t *cfg_views = ctx->cfg_views;
    
    printf("=== Multi-view Consistency Check  ===\n");
    
    for (int i = 0; i < ctx->view_cnt; i++) {
        if (res_views[i].score <= 0) {
            printf("View %d: skipped (no detection)\n", i);
            continue;
        }
        
        total_views++;
        //std::cout << "total_view cnt " << total_views << " i:" << i << std::endl;
        //std::cout << "cfg_views[" << i << "] " <<  cfg_views[i].view_id << std::endl;

        // 将3D位置投影回2D图像
        ctx->out_stream_id = i;
        //std::cout << "setting out_stream_id " << ctx->out_stream_id << std::endl;
        point_t projected_2d = get_out_img_coord(ctx, &estimated_3d_pos);
        
        //std::cout << "projectd_2d " << projected_2d.x << "," << projected_2d.y << std::endl;
        // 获取实际检测到的2D位置（中心点）
        point_t detected_2d = {
            res_views[i].rect.x + res_views[i].rect.w / 2,
            res_views[i].rect.y + res_views[i].rect.h / 2
        };
        
        // 计算重投影误差（归一化坐标下的距离）
        float reprojection_error = sqrt((projected_2d.x - detected_2d.x) * (projected_2d.x - detected_2d.x) +
                                       (projected_2d.y - detected_2d.y) * (projected_2d.y - detected_2d.y));
        
        total_reprojection_error += reprojection_error;
        
        // 设置误差阈值（根据图像尺寸调整）
        const float ERROR_THRESHOLD = 0.05f; // 3%的图像尺寸
        
        if (reprojection_error <= ERROR_THRESHOLD) {
            consistent_views++;
            printf("View %d: CONSISTENT (error=%.4f, threshold=%.4f)\n", 
                   i, reprojection_error, ERROR_THRESHOLD);
        } else {
            printf("View %d: INCONSISTENT (error=%.4f, threshold=%.4f)\n", 
                   i, reprojection_error, ERROR_THRESHOLD);
        }
        
        // 调试信息：显示详细的重投影结果
        printf("  Projected: (%.4f, %.4f), Detected: (%.4f, %.4f)\n",
               projected_2d.x, projected_2d.y, detected_2d.x, detected_2d.y);
    }
    
    if (total_views == 0) {
        printf("No valid views for consistency check\n");
        return 0;
    }
    
    // 计算平均重投影误差
    float avg_reprojection_error = total_reprojection_error / total_views;
    
    // 要求至少50%的视角一致，且平均误差不超过阈值
    float consistency_ratio = (float)consistent_views / total_views;
    const float MIN_CONSISTENCY_RATIO = 0.50f;
    const float MIN_AVG_ERROR = 0.05f;
    
    int is_consistent = (consistency_ratio >= MIN_CONSISTENCY_RATIO) && 
                       (avg_reprojection_error <= MIN_AVG_ERROR);
    
    printf("Multi-view consistency summary:\n");
    printf("  Consistent views: %d/%d (%.1f%%)\n", 
           consistent_views, total_views, consistency_ratio * 100);
    printf("  Average reprojection error: %.4f\n", avg_reprojection_error);
    printf("  Minimum consistency ratio: %.1f%%\n", MIN_CONSISTENCY_RATIO * 100);
    printf("  Maximum average error: %.4f\n", MIN_AVG_ERROR);
    printf("  Final result: %s\n", is_consistent ? "CONSISTENT" : "INCONSISTENT");
    
    return is_consistent;
}

void verify_coordinate_system(ball_map_ctx_t *ctx) {
    printf("=== Coordinate System Verification ===\n");
    
    // 验证四个角点的3D坐标
    for(int i=0; i<4; i++) {
        printf("Corner[%d]: (%.3f, %.3f, %.3f)\n", 
               i, ctx->points[i].x, ctx->points[i].y, ctx->points[i].z);
    }
    
    // 验证基础向量
    printf("Basis vectors:\n");
    printf("vec_x: (%.6f, %.6f, %.6f) len: %.3f\n", 
           ctx->vec_x.x, ctx->vec_x.y, ctx->vec_x.z, ctx->vec_x_len);
    printf("vec_y: (%.6f, %.6f, %.6f) len: %.3f\n", 
           ctx->vec_y.x, ctx->vec_y.y, ctx->vec_y.z, ctx->vec_y_len);
    printf("vec_z: (%.6f, %.6f, %.6f)\n", 
           ctx->vec_z.x, ctx->vec_z.y, ctx->vec_z.z);
    
    // 验证正交性
    printf("Orthogonality check:\n");
    printf("x·y: %.6f (should be ~0)\n", dot(ctx->vec_x, ctx->vec_y));
    printf("x·z: %.6f (should be ~0)\n", dot(ctx->vec_x, ctx->vec_z));
    printf("y·z: %.6f (should be ~0)\n", dot(ctx->vec_y, ctx->vec_z));
    
    // 验证向量长度
    printf("Vector lengths:\n");
    printf("|x|: %.6f (should be ~1.0)\n", norm(ctx->vec_x));
    printf("|y|: %.6f (should be ~1.0)\n", norm(ctx->vec_y));
    printf("|z|: %.6f (should be ~1.0)\n", norm(ctx->vec_z));
}

// 在 create_ball_map 函数中，替换现有的基础向量计算
static void orthonormalize_basis(com_vector_t &x, com_vector_t &y, com_vector_t &z) {
    // 确保X轴是单位向量
    x = unit(x);
    
    // 从Y轴中减去X轴方向的投影，使其与X轴正交
    double proj_y_on_x = dot(y, x);
    y.x -= proj_y_on_x * x.x;
    y.y -= proj_y_on_x * x.y;
    y.z -= proj_y_on_x * x.z;
    y = unit(y);
    
    // Z轴通过叉积得到（自动正交）
    z = cross(y, x);
    z = unit(z);
    
    printf("After orthonormalization:\n");
    printf("x·y: %.6f\n", dot(x, y));
    printf("x·z: %.6f\n", dot(x, z));
    printf("y·z: %.6f\n", dot(y, z));
}

ball_map_ctx_t *create_ball_map(ball_map_config_t cfg) {
    ball_map_ctx_t *ctx = new ball_map_ctx_t();
    if (!ctx) return NULL;
    ctx->cams = cfg.cams;
    ctx->cam_width  = ctx->cams[0].width;
    ctx->cam_height = ctx->cams[0].height;
    ctx->cfg_views = (ball_map_config_view_t*)malloc(sizeof(ball_map_config_view_t) * cfg.view_cnt);
    memcpy(ctx->cfg_views, cfg.cfg_views, sizeof(ball_map_config_view_t) * cfg.view_cnt);
    ctx->view_cnt = cfg.view_cnt;
    get_cam_coords(ctx, 0);
    get_cam_coords(ctx, 1);
    
    double min_z = ctx->points[0].z;
    for (int i = 1; i < 4; i++) {
        if (ctx->points[i].z < min_z) {
            min_z = ctx->points[i].z;
        }
    }
    std::cout << "=========== min_z original " << min_z << std::endl;
    

    com_vector_t vec_x = point_sub_point(ctx->points[1], ctx->points[0]);
    com_vector_t vec_y = point_sub_point(ctx->points[1], ctx->points[2]);
    float vec_x_len = norm(vec_x);
    float vec_y_len = norm(vec_y);
    ctx->vec_x = point_sub_point(ctx->points[2], ctx->points[3]);
    ctx->vec_y = point_sub_point(ctx->points[0], ctx->points[3]);
    ctx->vec_x = vec_add_vec(ctx->vec_x, vec_x);
    ctx->vec_y = vec_add_vec(ctx->vec_y, vec_y);
    ctx->vec_x_len = norm(ctx->vec_x);
    ctx->vec_y_len = norm(ctx->vec_y);

    ctx->vec_x_len /= 2;
    ctx->vec_y_len /= 2; //printf("x_len = %f, y_len = %f, unit_dist = %f, %f\n", vec_x_len, vec_y_len, vec_x_len / COURT_WIDTH, vec_y_len / COURT_HEIGHT);
    printf("vec_x = (%f, %f, %f), (%f, %f, %f)\n", ctx->vec_x.x, ctx->vec_x.y, ctx->vec_x.z, vec_x.x, vec_x.y, vec_x.z);
    ctx->vec_x = unit(ctx->vec_x);
    ctx->vec_y = unit(ctx->vec_y);
    ctx->vec_z = cross(ctx->vec_y, ctx->vec_x);
    //ctx->vec_z = cross(ctx->vec_x, ctx->vec_y);
    //ctx->vec_z = unit(ctx->vec_z);
    //if(ctx->vec_z.z < 0){
    //    ctx->vec_z = scale(ctx->vec_z, -1.0);
    //}
    ctx->avg_ball_width = ctx->avg_ball_height = 0;
    ctx->detect_count = 0;
    ctx->out_stream_id = cfg.out_stream_id;

    float plane_x_len = (distance(cfg.norm_corner_points[0], cfg.norm_corner_points[1]) + distance(cfg.norm_corner_points[2], cfg.norm_corner_points[3])) / 2;
    float plane_y_len = (distance(cfg.norm_corner_points[1], cfg.norm_corner_points[2]) + distance(cfg.norm_corner_points[0], cfg.norm_corner_points[3])) / 2;
    std::cout << "+++========== plane_x_len " << plane_x_len << " plane_y_len " << plane_y_len << std::endl;
    //float x_unit = ctx->vec_x_len / COURT_WIDTH * 2 / plane_x_len;
    //float y_unit = ctx->vec_y_len / COURT_HEIGHT * 2 / plane_y_len;
    float x_unit = ctx->vec_x_len / COURT_WIDTH;  // 每米对应的向量长度
    float y_unit = ctx->vec_y_len / COURT_HEIGHT; // 每米对应的向量长度
    
    // 考虑透视变换的影响
    x_unit /= plane_x_len;
    y_unit /= plane_y_len;
    std::cout << "ctx->vec_x_len: " << ctx->vec_x_len << " ctx->vec_y_len:" << ctx->vec_y_len << std::endl;
    float scale_x = x_unit; // * 0.225f;
    float rim_z[2];
    for (int i = 0; i < 2; i++) {
        point_t rim0 = get_out_img_coord(ctx, &ctx->rim_points[i]);
        com_vector_t vec;
        if (i == 0)
            vec = scale(ctx->vec_x, scale_x);
        else
            vec = scale(ctx->vec_x, -scale_x);
        ctx->rim_points[i] = point_add_vec(ctx->rim_points[i], vec);
        point_t rim1 = get_out_img_coord(ctx, &ctx->rim_points[i]);
        com_vector_t vec_tmp = point_sub_point(ctx->rim_points[i], ctx->points[3]);
        rim_z[i] = fabs(dot(vec_tmp, ctx->vec_z));
        printf("rim %d: (%d, %d)-(%d, %d), z = %f\n", i, (int)(rim0.x * IMG_WIDTH), (int)(rim0.y * IMG_HEIGHT), (int)(rim1.x * IMG_WIDTH), (int)(rim1.y * IMG_HEIGHT), rim_z[i]);
    }
    float z_unit = (rim_z[0] + rim_z[1]) / 2 / 1.524f;
    //float z_unit = 0.0;
    printf("x_len = %f, y_len = %f, unit_dist = (%f, %f, %f), plane_x_len = %f, plane_y_len = %f\n", ctx->vec_x_len, ctx->vec_y_len, x_unit, y_unit, z_unit, plane_x_len, plane_y_len);
    ctx->cam_unit_x = x_unit;
    ctx->cam_unit_y = y_unit;
    ctx->cam_unit_z = z_unit;

    cv::Point2f src_points[4] = {
        cv::Point2f(0, 1.0f),
        cv::Point2f(1.0f, 1.0f),
        cv::Point2f(1.0f, 0),
        cv::Point2f(0, 0)
    };
 
    printf("++== cfg.norm_corner_points[0].x %f, cfg.norm_corner_points[0].y %f\n", cfg.norm_corner_points[0].x, cfg.norm_corner_points[0].y);
    cv::Point2f dst_points[4] = {
        cv::Point2f(cfg.norm_corner_points[0].x, cfg.norm_corner_points[0].y),
        cv::Point2f(cfg.norm_corner_points[1].x, cfg.norm_corner_points[1].y),
        cv::Point2f(cfg.norm_corner_points[2].x, cfg.norm_corner_points[2].y),
        cv::Point2f(cfg.norm_corner_points[3].x, cfg.norm_corner_points[3].y)
    };

    ctx->plane_2_radar_matrix = cv::getPerspectiveTransform(src_points, dst_points);
    return ctx;
}


void destroy_ball_map(ball_map_ctx_t* map) {
    if (map) {
        delete map;
        free(map->cfg_views);
    }
}

static int ball_coord_convert(ball_map_ctx_t *ctx, int view0, int view1, rect_t *ball0, rect_t *ball1, ball_coord_t *out) {
    point_t pt0 = {ball0->x + ball0->w / 2, ball0->y + ball0->h / 2};
    point_t pt1 = {ball1->x + ball1->w / 2, ball1->y + ball1->h / 2};

    //std::cout <<"ball_coord_convert pt0 " << pt0.x << "," << pt0.y << std::endl;
    //std::cout << "pt1 " << pt1.x << "," << pt1.y << std::endl;
    com_point_t focus = calcu_cam_coord(ctx, pt0, view0, pt1, view1);
    //std::cout << "focus " << focus.x << "," << focus.y << "," << focus.z << std::endl;
    //std::cout << "view0 " << view0 << " view1 " << view1 << std::endl;

    // º∆À„ focus‘⁄«Ú≥°◊¯±Íœµx∫Õy∑ΩœÚµƒÕ∂”∞
    //std::cout << "ctx->point[3] " << ctx->points[3].x << "," << ctx->points[3].y << "," << ctx->points[3].z << std::endl;
    com_vector_t vec_focus = point_sub_point(focus, ctx->points[3]);
    //std::cout << "vec_focus " << vec_focus.x << "," << vec_focus.y << "," << vec_focus.z << std::endl;
    float length_x = dot(vec_focus, ctx->vec_x);
    float length_y = dot(vec_focus, ctx->vec_y);

    length_x /= ctx->vec_x_len;
    length_y /= ctx->vec_y_len;

    out->pos_2d.x = length_x;
    out->pos_2d.y = length_y;
    out->pos_3d = focus;
    //printf("{(%d, %.4f, %.4f), (%d, %.4f, %.4f)} -> (%.4f, %.4f)\n", view0, pt0.x, pt0.y, view1, pt1.x, pt1.y, out->pos_2d.x, out->pos_2d.y);
    return 0;
}

static inline void get_avg_coord(vector<ball_coord_t> &points, ball_coord_t *pt) {
    float x = 0;
    float y = 0;
    int size = points.size();
    if (size == 0) return;
    com_point_t pos_3d = {0, 0, 0};
    for (int i = 0; i < size; i++) {
        x += points[i].pos_2d.x;
        y += points[i].pos_2d.y;
        pos_3d.x += points[i].pos_3d.x;
        pos_3d.y += points[i].pos_3d.y;
        pos_3d.z += points[i].pos_3d.z;
    }
    pt->pos_2d.x = x / size;
    pt->pos_2d.y = y / size;
    pt->pos_3d.x = pos_3d.x / size;
    pt->pos_3d.y = pos_3d.y / size;
    pt->pos_3d.z = pos_3d.z / size;
}
static inline int get_ball_coord(vector<vector<ball_coord_t>> &map_points, ball_coord_t *pt_out, cv::Mat plane_2_radar_matrix) {
    size_t max_size = 0;
    int ret = -1;
    ball_coord_t pt;
    //std::cout << "get_ball_coord::map_points.size() " << map_points.size() << std::endl;
    for (size_t i = 0; i < map_points.size(); i++) {
        //std::cout << "map_point[" << i <<"].size() "<< map_points[i].size() << std::endl;
        if (map_points[i].size() > max_size) {
            get_avg_coord(map_points[i], &pt);
            ret = 0;
            //std::cout << "come input map_point size " << pt << " ret "<< ret<< std::endl;
            max_size = map_points[i].size();
        }
    }
    //std::cout << "get_ball_coord pt " << pt.pos_3d.x << "," << pt.pos_3d.y << "," << pt.pos_3d.z << std::endl;
    //std::cout << "!ret " << !ret << std::endl;
    if (!ret) {
        vector<cv::Point2f> src_points, dst_points;
        src_points.push_back(cv::Point2f(pt.pos_2d.x, pt.pos_2d.y));
        perspectiveTransform(src_points, dst_points, plane_2_radar_matrix);
        pt_out->pos_2d.x = dst_points[0].x;//(pt.pos_2d.x - 0.5f) * 2;
        pt_out->pos_2d.y = dst_points[0].y;//(0.5f - pt.pos_2d.y) * 2;
        pt_out->pos_3d = pt.pos_3d;
        //printf(" out{%f, %f} -> {%f, %f}\n", pt.x, pt.y, pt_out->x, pt_out->y);
    }
    return ret;
}
static void print_map_points(vector<vector<ball_coord_t>> map_points) {
    printf("map_points: ");
    for (size_t i = 0; i < map_points.size(); i++) {
        for (size_t j = 0; j < map_points[i].size(); j++) {
            printf("{(%.4f, %.4f), (%.4f, %.4f, %.4f)}, ", map_points[i][j].pos_2d.x, map_points[i][j].pos_2d.y, map_points[i][j].pos_3d.x, map_points[i][j].pos_3d.y, map_points[i][j].pos_3d.z);
        }
        printf("\n");
    }
}
static inline bool is_the_same_point(vector<ball_coord_t> &points, ball_coord_t &pt) {
    const float err_thresh = 0.02f;
    for (size_t i = 0; i < points.size(); i++) {
        point_t *tmp_pt = &points[i].pos_2d;
        if (abs(tmp_pt->x - pt.pos_2d.x) + abs(tmp_pt->y - pt.pos_2d.y) > err_thresh) {
            return false;
        }
    }
    return true;
}
static inline void insert_to_map_points(vector<vector<ball_coord_t>> &map_points, ball_coord_t &pt) {
    for (size_t i = 0; i < map_points.size(); i++) {
        if (is_the_same_point(map_points[i], pt)) {
            map_points[i].push_back(pt);
            return;
        }
    }
    vector<ball_coord_t> points = {pt};
    map_points.push_back(points);
}



int get_ball_coord_by_views(ball_map_ctx_t *ctx, ball_result_t *res_views, ball_info_t *out) {
    int view_cnt = ctx->view_cnt;
    ball_map_config_view_t *cfg_views = ctx->cfg_views;
    ball_result_t *ball_out = &out->rect;
    vector<vector<ball_coord_t>> map_points;
    //for (int i = 0; i < view_cnt; i++) {
    //    rect_t *rt = &res_views[i].rect;
    //    printf("{%d, (%.0f, %.0f)}, ", views[i], rt->x, rt->y);
    //}
    //printf("\n");
    float avg_w = 0;
    float avg_h = 0;
    float det_count = 0;
    int out_stream = ctx->out_stream_id;
    for (int i = 0; i < view_cnt; i++) {
        if (res_views[i].score <= 0)
            continue;
        int view0 = cfg_views[i].view_id;
        rect_t *rt0 = &res_views[i].rect;
        //std::cout << "view0 " << view0 << " rect " << rt0->x << "," << rt0->y << "," << rt0->w << "," << rt0->h << std::endl;
        det_count++;
        avg_w += rt0->w;
        avg_h += rt0->h;
        for (int j = i + 1; j < view_cnt; j++) {
            if (res_views[j].score <= 0)
                continue;
            int view1 = cfg_views[j].view_id;
            ball_coord_t coord = {{0, 0}, {0, 0, 0}};
            rect_t *rt1 = &res_views[j].rect;
            //std::cout << "view1 " << view1 << " rect " << rt1->x << "," << rt1->y << ","<< rt1->w << "," << rt1->h << std::endl;

            ball_coord_convert(ctx, view0, view1, rt0, rt1, &coord);
            insert_to_map_points(map_points, coord);
            //printf("{%d, %d, %.4f, %.4f}, ", view0, view1, coord.pos_2d.x, coord.pos_2d.y);
        }
    }
    //print_map_points(map_points);
    int ret = get_ball_coord(map_points, &out->pos, ctx->plane_2_radar_matrix);
    //std::cout << "ball_out " << ball_out << ", ret " << ret << " !ret " << !ret << std::endl;
    if (ball_out && !ret) {
        // ========== 在这里添加多视角一致性检查 ==========
        int multi_view_consistent = check_multi_view_consistency(ctx, res_views, out->pos.pos_3d);
        
        if (!multi_view_consistent) {
            printf("WARNING: Multi-view consistency check failed!\n");
            // 可以降低置信度或标记为可疑
            if (ball_out->score > 0) {
                ball_out->score *= 0.7f; // 降低置信度
            }
        }
        
        // ========== 在这里添加综合几何一致性检查 ==========
        //int boundary_valid = check_geometry_consistency(ctx, out, &prev_ball_info);
        //// 如果几何检查失败，但其他条件满足，可以标记为可疑或进行修正
        //if (boundary_valid) {
        //    printf("WARNING: Ball position failed geometry consistency check!\n");
        //    // 可以选择性地将分数降低或标记为不可用
        //    ball_out->score *= 0.5f; // 降低置信度
        //}
        
        
        // 综合所有检查结果
        int final_valid = multi_view_consistent;
        out->valid_geometry = final_valid;
        
        //if (!final_valid) {
        //    printf("WARNING: Ball position failed comprehensive geometry checks!\n");
        //    out->filtered = 1;
        //    out->avail = 0;
        //    return -1;
        //}

        if (det_count > 0) {
            avg_w /= det_count;
            avg_h /= det_count;
        }

        point_t pic_pos = get_out_img_coord(ctx, &out->pos.pos_3d);
        rect_t rt_map;
        float w, h;
        if (ctx->avg_ball_width > 0) {
            w = ctx->avg_ball_width;
            h = ctx->avg_ball_height;
        } else {
            w = avg_w;
            h = avg_h;
        }
        rt_map.x = pic_pos.x - w / 2;
        rt_map.y = pic_pos.y - h / 2;
        rt_map.w = w;
        rt_map.h = h;
        if (res_views[out_stream].score <= 0) {
            ball_out->score = 1.01f;
            ball_out->rect = rt_map;
        } else {
            //assert(false);
            rect_t *rt_det = &res_views[out_stream].rect;
            const float err_thresh = 0.02f;
            float dist = abs(rt_map.x + rt_map.w / 2 - rt_det->x - rt_det->w / 2) + abs(rt_map.y + rt_map.h / 2 - rt_det->y - rt_det->h / 2);
            if (dist > err_thresh) { // refine detect result
            	 ball_out->score = 1.01f;
            	 ball_out->rect = rt_map;
            } else {
            	*ball_out = res_views[out_stream];
               ctx->avg_ball_width = (ctx->avg_ball_width * ctx->detect_count + ball_out->rect.w) / (ctx->detect_count + 1);
               ctx->avg_ball_height = (ctx->avg_ball_height * ctx->detect_count + ball_out->rect.h) / (ctx->detect_count + 1);
               if (++ctx->detect_count > 31)
                   ctx->detect_count = 31;
            }
        }
        
        // 保存当前帧数据用于下一帧的连续性检查
        prev_ball_info = *out;
 
        calcu_ball_court_center_offset(ctx, out);
        //calcu_ball_rim_vectors(ctx, out);
        out->avail = 1;
        printf("Final validation: boundary=%d, multi-view=%d, overall=%s\n",
               out->valid_geometry, multi_view_consistent,
               final_valid ? "VALID" : "INVALID");
        //printf("remaped ball: %.1f, %.1f, %.1f, %.1f\n", rt_out->x, rt_out->y, rt_out->w, rt_out->h);
    } else {
        out->avail = 0;
    }
    return ret;
}

